/*
 * IDA* is a good improvement over A*
 */
#include <stdio.h>
#include <queue>
#include <list>
#include <vector>
#include <map>

#define INF 0x7fffffff

struct Edge {
  Edge(int v, int dist) : v(v), dist(dist) { }
  Edge() : v(-1), dist(INF) { }

  int v;
  int dist;
};

typedef std::list<Edge> EdgeList;

class Graph {
public:
  Graph(int n_vertices) : vertices(n_vertices) { }
  void AddEdge(int i, int j, int dist) {
    vertices[i].push_back(Edge(j, dist));
    vertices[j].push_back(Edge(i, dist));
  }
  int n_vertices() const { return vertices.size(); }
  const EdgeList &vertex(int i) const { return vertices[i]; }
private:
  std::vector<EdgeList> vertices;
};

typedef std::list<int> OpenSet;
typedef std::vector<bool> ClosedSet;
typedef std::vector<int> ScoreSet;

class HeuristicCost {
public:
  HeuristicCost(int n) : costs(n, INF) { }
  int Evaluate(int i, int j) const { return costs[i]; }
  void SetCost(int i, int j, int cost) { costs[i] = cost; }
private:
  std::vector<int> costs;
};

OpenSet::iterator FindVertexWithLowestFScore(OpenSet &q, const ScoreSet &fscore) {
  if (q.empty()) return q.end();
  OpenSet::iterator k = q.begin();
  int dist = fscore[*k];
  for (OpenSet::iterator it = q.begin(); it != q.end(); ++it) {
    if (fscore[*it] < dist) {
      k = it;
      dist = fscore[*it];
    }
  }
  return k;
}

class Marker {
public:
  Marker(int n) : marker_(n, 0) { }

  bool InOpenSet(int i) const { return (marker_[i] & 2) != 0; }
  void SetInOpenSet(int i) { marker_[i] |= 2; }
  void ResetInOpenSet(int i) { marker_[i] &= 0xfffffffd; }

  bool InClosedSet(int i) const { return (marker_[i] & 1) != 0; }
  void SetInClosedSet(int i) { marker_[i] |= 1; }
private:
  std::vector<char> marker_;
};

void ReconstructPath(const std::vector<int> &previous, int i, int j) {
  std::list<int> path;
  int u = j;
  while (previous[u] >= 0) {
    path.push_front(u);
    u = previous[u];
  }
  path.push_front(i);

  for (std::list<int>::const_iterator it = path.begin(); it != path.end(); ++it) {
    printf("%d ", *it);
  }
}

void AStar(const Graph &g, int i, int j, const HeuristicCost &hcost) {
  Marker marker(g.n_vertices());

  OpenSet openset;
  openset.push_back(i);
  marker.SetInOpenSet(i);

  ScoreSet g_score(g.n_vertices(), INF);
  ScoreSet f_score(g.n_vertices());
  std::vector<int> previous(g.n_vertices(), -1);
  
  g_score[i] = 0;
  f_score[i] = g_score[i] + hcost.Evaluate(i, j);

  while (!openset.empty()) {
    OpenSet::iterator current = FindVertexWithLowestFScore(openset, f_score);
    if (*current == j) {
      ReconstructPath(previous, i, j);
      return;
    }
    marker.SetInClosedSet(*current);
    for (EdgeList::const_iterator it = g.vertex(*current).begin();
         it != g.vertex(*current).end(); ++it) {
      if (marker.InClosedSet(it->v)) continue;
      int tentative_g_score = g_score[*current] + it->dist;
      if (!marker.InOpenSet(it->v)) {
        openset.push_back(it->v);
        marker.SetInOpenSet(it->v);
      }
      if (tentative_g_score < g_score[it->v]) {
        previous[it->v] = *current;
        g_score[it->v] = tentative_g_score;
        f_score[it->v] = g_score[it->v] + hcost.Evaluate(it->v, j);
      }
    }
    marker.ResetInOpenSet(*current);
    openset.erase(current);
  }
  // failure
}

int main(int argc, const char *argv[]) {
  Graph g(6);
  g.AddEdge(0, 1, 7);
  g.AddEdge(0, 2, 9);
  g.AddEdge(0, 5, 14);
  g.AddEdge(1, 2, 10);
  g.AddEdge(1, 3, 15);
  g.AddEdge(2, 3, 11);
  g.AddEdge(2, 5, 2);
  g.AddEdge(3, 4, 6);
  g.AddEdge(4, 5, 9);

  HeuristicCost hcost(g.n_vertices());

  int goal = 4;
  for (EdgeList::const_iterator it = g.vertex(goal).begin();
       it != g.vertex(goal).end(); ++it) {
    hcost.SetCost(it->v, goal, it->dist);
  }

  AStar(g, 0, goal, hcost);
  
  return 0;
}

